#ifndef CAR_HPP
#define CAR_HPP

#define SHOWSEGMENT

#include "digit.hpp"

#include "usbcamera.hpp"

#include "tost.hpp"

#include "server.hpp"

#include "iscanner.hpp"
#include <string>


#define IMSHOWSTATUS

class Car : public ServerBase
{
public:

    static cv::Vec3d pt2rayf(cv::Vec2d pt, double angle, const cv::Mat &K, const cv::Mat &D)
    {
        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=angle;
        cv::Rodrigues(rv, R);

        std::vector<cv::Vec2d> inl(1, pt);
        std::vector<cv::Vec2d> outl;
        cv::fisheye::undistortPoints(inl,outl, K, D, R);

        return cv::Vec3d(outl[0][0], outl[0][1], 1);
    }


    static cv::Vec3d pt2ray(cv::Vec2d pt, double angle, const cv::Mat &K, const cv::Mat &D)
    {
        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=angle;
        cv::Rodrigues(rv, R);

        std::vector<cv::Vec2d> inl(1, pt);
        std::vector<cv::Vec2d> outl;
        cv::undistortPoints(inl,outl, K, D, R);

        return cv::Vec3d(outl[0][0], outl[0][1], 1);
    }


    static cv::Vec3d ray2center(cv::Vec3d ray, double yheight)
    {
        return (yheight/ray[1])*ray;
    }





    static void gettrans(cv::Vec3d p1, cv::Vec3d p2, cv::Vec3d &d, double &l)
    {
        cv::Vec2d dp(p2[0]-p1[0],p2[2]-p1[2]);
        double theta=atan(-dp[1]/dp[0]);
        l=dp[0]*cos(theta)-dp[1]*sin(theta);

        cv::Matx22d r(cos(theta), -sin(theta), sin(theta), cos(theta));

        cv::Vec2d dd=cv::Vec2d(l/2,0)-r*cv::Vec2d(p2[0],p2[2]);
        d[0]=dd[0];
        d[1]=dd[1];
        d[2]=theta;

        //        std::cout<<"gettrans="<<cv::Vec2d(-l/2,0)-r*cv::Vec2d(p1[0],p1[2])-dd<<"\n";
    }

    static cv::Vec2d targetcar(cv::Vec2d xz, cv::Vec3d dd)
    {
        double theta=dd[2];
        cv::Matx22d r(cos(theta), -sin(theta), sin(theta), cos(theta));

        return r.inv()*(xz-cv::Vec2d(dd[0],dd[1]));

    }



    static void FixRadDiff(double &rad)
    {
        while(rad>CV_PI) rad-=CV_2PI;
        while(rad<=-CV_PI) rad+=CV_2PI;
    }

    static void drawv(cv::Mat &canvas, cv::Vec3d v, int r)
    {
        char buf[64];
        sprintf(buf,"%g,%g,%g",v[0],v[1],v[2]);
        cv::putText(canvas, buf, cv::Point(0,r),
                    CV_FONT_HERSHEY_SIMPLEX, 0.5,
                    cv::Scalar(255,255,255));
    }

    static cv::Vec3d PointsToCarPos(cv::Vec3d p1, cv::Vec3d p2)
    {
        cv::Vec3d ddd;
        double ll;
        gettrans(p1,p2,ddd,ll);
        return ddd;
    }

    static cv::Vec3d DiffToTarget(cv::Vec3d car, cv::Vec3d target)
    {
        cv::Vec2d tacar=targetcar(cv::Vec2d(target[0],target[1]), car);

        cv::Vec3d tac(tacar[0],tacar[1],car[2]-target[2]);

        FixRadDiff(tac[2]);

        return tac;
    }

    static double interp(double x, double xmin, double xmax, double ymin, double ymax)
    {
        if(x<=xmin)
            return ymin;
        if(x>=xmax)
            return ymax;
        return ymin+(ymax-ymin)*(x-xmin)/(xmax-xmin);
    }


    static cv::Vec3d ToSpeed(cv::Vec3d diff,
                             double maxMove=50, double maxRotate=0.2,
                             double xzThres=50, double radThres=0.04)
    {
        cv::Vec3d speed(0,0,0);
        double absxz=sqrt(diff[0]*diff[0]+diff[1]*diff[1]);

        double vMove=interp(absxz, xzThres, maxMove*2, 40, maxMove);

        double rMove=interp(fabs(diff[2]), radThres, maxRotate*2, 0.05, maxRotate);

        speed[0]=vMove*diff[0]/absxz;
        speed[1]=vMove*diff[1]/absxz;
        if(diff[0]<xzThres && diff[0]>-xzThres)
            speed[0]=0;
        if(diff[1]<xzThres && diff[1]>-xzThres)
            speed[1]=0;

        if(diff[2]>radThres)
            speed[2]=-rMove;
        else
            if(diff[2]<-radThres)
                speed[2]=rMove;
            else
                speed[2]=0;

        return speed;
    }

    static double LowPass(double oldValue, double inputValue, double factor=0.1)
    {
        return oldValue*(1-factor)+inputValue*factor;
    }


    static void CarMove(cv::Vec3d target, USBCamera &c, ToST &tst,
                        cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> &ocr1,
                        const cv::Mat &Knew, const cv::Mat &map1, const cv::Mat &map2,
                        unsigned num=45, int thres=170)
    {

        double maxdmm=7000;

        double brandBottom=1630;
        double brandHalfHeight=200;
        double cameraBottom=1094;
        double cameraCenterHeight=31;

        double maxVxz=250;
        double maxVr=0.4;
        double errorxz=30;
        double errorr=0.03;

        double hmm=brandBottom-cameraBottom-cameraCenterHeight;
        int h=Knew.at<double>(1,2)-Knew.at<double>(0,0)/maxdmm*hmm;

        int k=0;
        cv::Mat mm;
        cv::Mat mmu;

        cv::Rect nbb[2];

        int lostCount=0;

        int i=0;

        cv::Vec3d speedout(0,0,0);

        cv::Size gap(30,30);

        double resp=0;
        while(c.read(mm) && k!=27)
        {
            int64 t0=cv::getTickCount();

            std::cout<<i<<"\n"<<std::flush;

            cv::remap(mm, mmu, map1.rowRange(0,h), map2.rowRange(0,h), cv::INTER_LINEAR);

//              std::cout<<i<<"remap\n"<<std::flush;

            if(resp<0.1)
                resp=Digit::LocateTwoDigit(mmu, num, ocr1, nbb, thres);
            else
                resp=Digit::TraceTwoDigit(mmu, num, ocr1, nbb, thres, gap);

//             std::cout<<i<<"digit\n"<<std::flush;

            if(resp<0.1)
            {
                speedout[0]=LowPass(speedout[0],0, 0.1);
                speedout[1]=LowPass(speedout[1],0, 0.1);
                speedout[2]=LowPass(speedout[2],0, 0.1);

                lostCount++;
                if(lostCount>10)
                    tst.Move();
                else
                    tst.Move(speedout[0],speedout[1],speedout[2]);

                int64 t1=cv::getTickCount();

                std::cout<<"\nfail="<<(double)(t1-t0)/cv::getTickFrequency()<<std::flush;

                cv::imshow("canvas",mmu.colRange(mmu.cols/4,mmu.cols*3/4));
            }
            else
            {
                lostCount=0;

                int64 t1=cv::getTickCount();

                std::cout<<"\ndone="<<(double)(t1-t0)/cv::getTickFrequency()<<std::flush;

                cv::Vec2d pt1(nbb[0].x+nbb[0].width/2, nbb[0].y+nbb[0].height/2);
                cv::Vec2d pt2(nbb[1].x+nbb[1].width/2, nbb[1].y+nbb[1].height/2);

                cv::Vec3d ct1=ray2center(pt2ray(pt1, 0, Knew,cv::Mat()), -(hmm+brandHalfHeight));
                cv::Vec3d ct2=ray2center(pt2ray(pt2, 0, Knew,cv::Mat()), -(hmm+brandHalfHeight));

                cv::Vec3d xza=PointsToCarPos(ct1,ct2);

                gap.width=gap.height=20+(-xza[1]-6000)*(60-20)/(1000-6000);

                target[0]=(xza[0]<1100?(xza[0]>-1100?xza[0]:-1100):1100);

                cv::Vec3d tdf=DiffToTarget(xza, target);

//                if(xza[0]*xza[0]+xza[1]*xza[1]>4000*4000)
//                 {
//                    tdf[0]=0;
//                    tdf[2]=0;
//                }


                cv::Vec3d speed=ToSpeed(tdf,maxVxz,maxVr,errorxz,errorr);

                speedout[0]=LowPass(speedout[0],speed[0]);
                speedout[1]=LowPass(speedout[1],speed[1]);
                speedout[2]=LowPass(speedout[2],speed[2]);

                if(tdf[0]*tdf[0]+tdf[1]*tdf[1]<errorxz*errorxz && fabs(tdf[2])<errorr)
                {
                    tst.Move();
                    break;
                }
                else
                {
                    tst.Move(speedout[0],speedout[1],speedout[2]);
                }


#ifdef IMSHOWSTATUS
                cv::Mat dfc(240,320,CV_8UC3,cv::Scalar(0));

                drawv(dfc, ct1, 30);
                drawv(dfc, ct2, 60);
                drawv(dfc, xza, 90);
                drawv(dfc, tdf, 120);
                drawv(dfc, speed, 150);
                drawv(dfc, speedout, 180);

                cv::imshow("status", dfc);
#endif


                cv::rectangle(mmu, nbb[0], cv::Scalar(0,0,255), 5);
                cv::rectangle(mmu, nbb[1], cv::Scalar(0,255,0), 5);
                cv::imshow("canvas",mmu.colRange(mmu.cols/4, mmu.cols*3/4));

            }
            k=cv::waitKey(21);
            i++;

        }

    }





    static void CarMoveP(cv::Vec3d target, USBCamera &c, ToST &tst,
                         cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> &ocr1,
                         const cv::Mat &Knew, const cv::Mat &map1, const cv::Mat &map2,
                         double pBottom, cv::Scalar hsvThres[2])
    {

        double maxdmm=2000;

        //        double pBottom=1210+25;
        //        double pBottom=1170+30+25;

        double cameraBottom=1090;
        double cameraCenterHeight=31;

        double maxVxy=350;
        double maxVr=2.5;
        double errorxy=60;
        double errorr=0.03;


        double hmm=pBottom-cameraBottom-cameraCenterHeight;
        int h=Knew.at<double>(1,2)-Knew.at<double>(0,0)/maxdmm*hmm+50;

        cv::Vec2d cropratio(0.2,0.8);
        cv::Rect roi(map1.cols*(1-cropratio[0])/2, map1.rows*(1-cropratio[1])/2, map1.cols*cropratio[0], map1.rows*cropratio[1]);

        if(roi.y+roi.height>h)
            roi.height=h-roi.y;



        int k=0;
        cv::Mat mm;
        cv::Mat mmu;


        cv::Rect nbb[2];
        cv::Vec2d co[4];

        int i=0;


        int lostCount=0;



        cv::Vec3d speedout(0,0,0);

        double resp=0;
        while(c.read(mm) && k!=27)
        {
            int64 t0=cv::getTickCount();

            std::cout<<i<<"\n"<<std::flush;

            cv::remap(mm, mmu, map1(roi), map2(roi), cv::INTER_LINEAR);
            //            cv::remap(mm, mmu, map1, map2, cv::INTER_LINEAR);

            if(resp<0.5)
                resp=Digit::LocateP(mmu, ocr1, nbb[0], co, hsvThres);
            else
                resp=Digit::TraceP(mmu, ocr1, nbb[0], co, hsvThres, cv::Size(25,25));

            for(int j=0;j<4;j++)
            {
                co[j][0]+=roi.x;
                co[j][1]+=roi.y;
            }



            if(resp<0.5)
            {

                speedout[0]=LowPass(speedout[0],0, 0.1);
                speedout[1]=LowPass(speedout[1],0, 0.1);
                speedout[2]=LowPass(speedout[2],0, 0.1);

                lostCount++;
                if(lostCount>10)
                    tst.Move();
                else
                    tst.Move(speedout[0],speedout[1],speedout[2]);

                int64 t1=cv::getTickCount();

                std::cout<<"\nfail="<<(double)(t1-t0)/cv::getTickFrequency()<<std::flush;

                cv::imshow("canvas", mmu);
            }
            else
            {

                lostCount=0;

                int64 t1=cv::getTickCount();

                std::cout<<"\ndone="<<(double)(t1-t0)/cv::getTickFrequency()<<std::flush;

                cv::Vec3d ct1=ray2center(pt2ray(co[1], 0, Knew,cv::Mat()), -(hmm));
                cv::Vec3d ct2=ray2center(pt2ray(co[2], 0, Knew,cv::Mat()), -(hmm));

                cv::Vec3d xza=PointsToCarPos(ct1,ct2);

                cv::Vec3d tdf=DiffToTarget(xza, target);

                if(xza[0]*xza[0]+xza[1]*xza[1]>1000*1000)
                {
                    tdf[2]=0;
                    if(target[0]*target[0]+target[1]*target[1]>700*700)
                    {
                        tdf[0]=0;
                    }
                }

                cv::Vec3d speed=ToSpeed(tdf, maxVxy, maxVr, errorxy, errorr);

                speedout[0]=LowPass(speedout[0],speed[0]);
                speedout[1]=LowPass(speedout[1],speed[1]);
                speedout[2]=LowPass(speedout[2],speed[2]);

#ifdef IMSHOWSTATUS
                cv::Mat dfc(240,320,CV_8UC3,cv::Scalar(0));

                drawv(dfc, ct1, 30);
                drawv(dfc, ct2, 60);
                drawv(dfc, xza, 90);
                drawv(dfc, tdf, 120);
                drawv(dfc, speed, 150);
                drawv(dfc, speedout, 180);

                std::cout<<"\nct1="<<ct1<<std::flush;
                std::cout<<"\nct2="<<ct2<<std::flush;
                std::cout<<"\nxza="<<xza<<std::flush;

                cv::imshow("status", dfc);

#endif



                if(tdf[0]*tdf[0]+tdf[1]*tdf[1]<errorxy*errorxy && fabs(tdf[2])<errorr)
                {
                    tst.Move();
                    break;
                }
                else
                {
                    tst.Move(speedout[0],speedout[1],speedout[2]);
                }

              cv::rectangle(mmu, nbb[0], cv::Scalar(0,0,255), 5);
                 cv::imshow("canvas",mmu);

                for(int j=0;j<4;j++)
                {
                    co[j][0]-=roi.x;
                    co[j][1]-=roi.y;
                }

                Digit::drawp(mmu, nbb[0], co);
            }
            k=cv::waitKey(31);
            i++;

        }

    }

    static void WaitSecond(USBCamera &c, double second)
    {
        int64 t0=cv::getTickCount()+second*cv::getTickFrequency();

        int64 t1=cv::getTickCount();

        while(t1<t0)
        {
            cv::Mat m;
            if(c.read(m))
            {
                cv::imshow("canvas", m);
                cv::waitKey(1);
            }
            t1=cv::getTickCount();
        }
    }

    static void R90(USBCamera &c, ToST &tst, bool left=true)
    {
        double rad=0.3;
        tst.Move(0,0,left?rad:-rad);

        //        double s=CV_PI/rad*0.1;
        //        std::cout<<"\nR90="<<s<<"\n"<<std::flush;
        //        std::this_thread::sleep_for(std::chrono::milliseconds(ms));
        WaitSecond(c, CV_PI/rad*0.1);

        rad=0.6;
        tst.Move(0,0,left?rad:-rad);
        WaitSecond(c, CV_PI/rad*0.329);

        rad=0.3;
        tst.Move(0,0,left?rad:-rad);
        WaitSecond(c, CV_PI/rad*0.1);

        tst.Move();


    }

    static void ParkTo10(USBCamera &c, ToST &tst,
                         cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> &ocr1,
                         const cv::Mat &Knew, const cv::Mat &map1, const cv::Mat &map2)
    {
        cv::Scalar hsvThres[2]={
//            cv::Scalar(150,-1,150), cv::Scalar(180,256,255)
//            cv::Scalar(-1,-1,110), cv::Scalar(256,256,255)
//            cv::Scalar(150,-1,100), cv::Scalar(20,256,255)
            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        cv::Scalar hsvThres1[2]={
//             cv::Scalar(172,-1,120), cv::Scalar(10,256,255)
//             cv::Scalar(-1,-1,95), cv::Scalar(256,256,255)
//            cv::Scalar(172,60,120), cv::Scalar(14,256,255)
//            cv::Scalar(152,50,120), cv::Scalar(2,256,255)
            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        int thres=230;
//        CarMoveP(cv::Vec3d(0,-1300,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres);
//        cv::waitKey(100);
//        R90(c,tst,false);
//        cv::waitKey(100);
        CarMove(cv::Vec3d(600,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2,10,thres);
        cv::waitKey(100);
//        R90(c,tst,true);
//        cv::waitKey(100);
//        CarMoveP(cv::Vec3d(0,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres1);
//        cv::waitKey(100);
        tst.Move();
    }
    static void ParkTo89(USBCamera &c, ToST &tst,
                         cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> &ocr1,
                         const cv::Mat &Knew, const cv::Mat &map1, const cv::Mat &map2)
    {
        cv::Scalar hsvThres[2]={
//            cv::Scalar(150,-1,150), cv::Scalar(180,256,255)
//            cv::Scalar(-1,-1,110), cv::Scalar(256,256,255)
//            cv::Scalar(150,-1,100), cv::Scalar(20,256,255)
            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        cv::Scalar hsvThres1[2]={
//            cv::Scalar(172,-1,120), cv::Scalar(10,256,255)
//             cv::Scalar(-1,-1,95), cv::Scalar(256,256,255)
//            cv::Scalar(172,60,120), cv::Scalar(14,256,255)
//            cv::Scalar(152,50,120), cv::Scalar(2,256,255)

            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        int thres=230;
//        CarMoveP(cv::Vec3d(0,-1300,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres1);
//        cv::waitKey(100);
//        R90(c,tst,true);
//        cv::waitKey(100);
        CarMove(cv::Vec3d(-600,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2,89,thres);
        cv::waitKey(100);
//        R90(c,tst,false);
//        cv::waitKey(100);
//        CarMoveP(cv::Vec3d(0,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres);
//        cv::waitKey(100);
        tst.Move();
    }
    static void ParkTo67(USBCamera &c, ToST &tst,
                         cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> &ocr1,
                         const cv::Mat &Knew, const cv::Mat &map1, const cv::Mat &map2)
    {
        cv::Scalar hsvThres[2]={
//            cv::Scalar(150,-1,150), cv::Scalar(180,256,255)
//            cv::Scalar(-1,-1,110), cv::Scalar(256,256,255)
//            cv::Scalar(150,-1,100), cv::Scalar(20,256,255)
            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        cv::Scalar hsvThres1[2]={
//            cv::Scalar(172,-1,120), cv::Scalar(10,256,255)
//             cv::Scalar(-1,-1,95), cv::Scalar(256,256,255)
//            cv::Scalar(172,60,120), cv::Scalar(14,256,255)
//            cv::Scalar(152,50,120), cv::Scalar(2,256,255)

            cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        int thres=230;
//        CarMoveP(cv::Vec3d(0,-1300,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres1);
//        cv::waitKey(100);
//        R90(c,tst,true);
//        cv::waitKey(100);
        CarMove(cv::Vec3d(-600,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2,67,thres);
        cv::waitKey(100);
//        R90(c,tst,false);
//        cv::waitKey(100);
//        CarMoveP(cv::Vec3d(0,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres);
//        cv::waitKey(100);
        tst.Move();
    }


    //#define TESTV


    static void test()
    {
        cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> ocr1 = cv::text::loadOCRHMMClassifierCNN("OCRBeamSearch_CNN_model_data.xml.gz");
#ifdef TESTV
        //        cv::VideoCapture c("TV_CAM_device_20170919_105606.mkv");

        //        cv::VideoCapture c("TV_CAM_device_20171011_092616.mkv");
        cv::VideoCapture c("TV_CAM_device_20171016_150352.mkv");
#else
        USBCamera c;
        c.open(0);
#endif

        cv::Mat K,D;
        USBCamera::Load(K, "Kf.yml");
        USBCamera::Load(D, "Df.yml");

        cv::Size sznew=cv::Size(1280,720)*4 ;

//       USBCamera::ScaleK(K, 1920./1280, 1080./720);

        cv::Mat Knew=K.clone();
        Knew.at<double>(0,2)=sznew.width/2;
        Knew.at<double>(1,2)=sznew.height/2;


        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=20*CV_PI/180;
        cv::Rodrigues(rv, R);


        int thres=230;


        cv::Mat map1, map2;
        cv::fisheye::initUndistortRectifyMap(K, D, R, Knew, sznew, CV_16SC2, map1, map2);

        cv::namedWindow("canvas",cv::WINDOW_NORMAL);
        cv::moveWindow("canvas", 0, 0);
        cv::resizeWindow("canvas",1920,1080);

#ifdef IMSHOWSTATUS
        cv::namedWindow("status",cv::WINDOW_NORMAL);
#endif


        cv::namedWindow("p",cv::WINDOW_NORMAL);
        cv::moveWindow("p", 0, 0);
        cv::resizeWindow("p", 300, 400);
#ifdef SHOWSEGMENT
        cv::namedWindow("bw",cv::WINDOW_NORMAL);
        cv::namedWindow("hsv",cv::WINDOW_NORMAL);
#endif

        cv::Scalar hsvThres[2]={
//            cv::Scalar(-1,-1,100), cv::Scalar(256,256,255)
//cv::Scalar(152,50,120), cv::Scalar(2,256,255)
             cv::Scalar(160,-1,100), cv::Scalar(30,256,255)
        };

        ToST tst;

        tst.Initial();

//          CarMoveP(cv::Vec3d(0,-1500,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres);

//          cv::waitKey(100);

//           R90(c,tst,false);


//                cv::waitKey(1000);
//                CarMove(cv::Vec3d(600,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2,23,thres);

//        R90(c,tst,false);
//        cv::waitKey(100);
//                R90(c,tst,false);

//        cv::waitKey(10000);
//                 CarMoveP(cv::Vec3d(-00,-500,CV_PI*0),c,tst,ocr1,Knew, map1,map2, 1200, hsvThres);

//               CarMove(cv::Vec3d(-600,-600,CV_PI*0),c,tst,ocr1,Knew, map1,map2,45,thres);

#if 1
//        ParkTo23(c,tst,ocr1,Knew,map1,map2);
//        cv::waitKey(3000);
        ParkTo89(c,tst,ocr1,Knew,map1,map2);
#endif

        cv::waitKey(100);
        tst.Move();
        tst.End();

        c.release();

    }

    static void ScanUnlock(USBCamera &c, ToST &tst)
    {
        IScanner ms;

        bool res=false;

        cv::Mat m;
        cv::Mat mg;

        std::vector<std::string> data;

        while(!res)
        {
            if(c.read(m))
            {
                cv::cvtColor(m,mg,CV_BGR2GRAY);
                res=ms.scan(mg,data);
                cv::imshow("canvas",m);
                cv::waitKey(1);
            }
        }

        std::cout<<data[0]<<"\n"<<std::flush;

        if(data[0].compare("http://graphite.org")==0)
        {
            std::cout<<"\nunlock 0\n"<<std::flush;
            tst.Unlock(1);
        }
        else
        {
            std::cout<<"\nunlock 1\n"<<std::flush;
            tst.Unlock(0);
        }

    }


public:
    USBCamera c;
    cv::Mat Knew;
    cv::Mat map1;
    cv::Mat map2;
    ToST tst;
    cv::Ptr<cv::text::OCRHMMDecoder::ClassifierCallback> ocr1;

    Car()
        : ServerBase()
    {

    }

    void start(uint16_t portno=7654)
    {
        ocr1 = cv::text::loadOCRHMMClassifierCNN("OCRBeamSearch_CNN_model_data.xml.gz");

        c.open(0);

        cv::Mat K,D;
        USBCamera::Load(K, "Kf.yml");
        USBCamera::Load(D, "Df.yml");

        cv::Size sznew=cv::Size(1280,720)*4 ;
        Knew=K.clone();
        Knew.at<double>(0,2)=sznew.width/2;
        Knew.at<double>(1,2)=sznew.height/2;


        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=20*CV_PI/180;
        cv::Rodrigues(rv, R);

        cv::fisheye::initUndistortRectifyMap(K, D, R, Knew, sznew, CV_16SC2, map1, map2);

        cv::namedWindow("canvas",cv::WINDOW_NORMAL);
        cv::moveWindow("canvas", 0, 0);
        cv::resizeWindow("canvas",1920,1080);

#ifdef IMSHOWSTATUS
        cv::namedWindow("status",cv::WINDOW_NORMAL);
#endif

        cv::namedWindow("p",cv::WINDOW_NORMAL);
        cv::moveWindow("p", 0, 0);
        cv::resizeWindow("p", 300, 400);
#ifdef SHOWSEGMENT
        cv::namedWindow("bw",cv::WINDOW_NORMAL);

        cv::namedWindow("hsv",cv::WINDOW_NORMAL);
#endif
        tst.Initial();

        ServerBase::start(portno);

        cv::waitKey(100);
        tst.Move();
        tst.End();

        c.release();

    }


    virtual void task(char buffer[256])
    {

        ServerBase::task(buffer);

        if(strncmp(buffer,"TO10",4)==0)
        {
            ParkTo10(c,tst,ocr1,Knew,map1,map2);
        }

        if(strncmp(buffer,"TO89",4)==0)
        {
            ParkTo89(c,tst,ocr1,Knew,map1,map2);
        }

        if(strncmp(buffer,"TO67",4)==0)
        {
            ParkTo67(c,tst,ocr1,Knew,map1,map2);
        }

        if(strncmp(buffer,"LOCK",4)==0)
        {
            ScanUnlock(c,tst);
        }

    }

};


#endif // DIGIT_HPP

